//speed_sensor.c
#include "speed_sensor.h"
#include "delay.h"  // 如果没有专用定时器
void speed_encoder_callback(void *sensor_ctx);
/*
**当interval_ms为0时，为mt测速模式，否则为m模式
*/
void speed_init(SpeedSensor *sensor,
                GPIO_PinName  ch_a_pin, GPIO_PinName  ch_b_pin,
                uint16_t ppr,
                uint32_t interval_ms)
{
	encoder_init(&sensor->encoder,  ch_a_pin, ch_b_pin);
    sensor->ppr = ppr;
	if(interval_ms == 0) {
		encoder_register_callback(&sensor->encoder, speed_encoder_callback, sensor);
		sensor->interval_sec = 0;
	} else {
		sensor->interval_sec = interval_ms / 1000.0f;
	}
    sensor->last_count = encoder_get_count(&sensor->encoder);
	sensor->last_micros = micros();
    sensor->rpm = 0.0f;
    sensor->rps = 0.0f;
}

void speed_encoder_callback(void *sensor_ctx) { //每次主循环之后的第一次编码器中断
	SpeedSensor *sensor = (SpeedSensor*) sensor_ctx;
	if(sensor->speed_updated) //如果不是第一次中断
		return;
	sensor->speed_updated = 1;
	
	int32_t current_count = encoder_get_count(&sensor->encoder);
    int32_t delta_count = current_count - sensor->last_count; //两次的编码器值差
    sensor->last_count = current_count;
	
	int32_t current = micros();
	int32_t delta_us = current - sensor->last_micros; //两次的微秒时间差
	sensor->last_micros = current;
	
	float dt =  delta_us/ 1.0e6f;
	
	if (dt > 0.0001f) {
		sensor->rps = delta_count / ((float)sensor->ppr * dt);
		sensor->rpm = sensor->rps * 60.0f;
	}
}


void speed_update(SpeedSensor *sensor)
{
	if(sensor->interval_sec == 0) { //mt测速模式
		if(!sensor->speed_updated) { //如果上个周期没有收到编码器信号
			if(micros() - sensor->last_micros > 1000000) { //超过一秒没收到
				sensor->rps = 0;
				sensor->rpm = 0;
			}
		}
		sensor->speed_updated = 0;
	} else { //m模式
		int32_t current = encoder_get_count(&sensor->encoder);
		int32_t delta = current - sensor->last_count;
		sensor->last_count = current;

		// 转/秒 = delta / (ppr * 秒)
		sensor->rps = delta / (float)(sensor->ppr * sensor->interval_sec);
		sensor->rpm = sensor->rps * 60.0f;
	}
}

float speed_get_rpm(SpeedSensor *sensor)
{
    return sensor->rpm;
}

float speed_get_rps(SpeedSensor *sensor)
{
    return sensor->rps;
}
